#ifndef CAMERA_SELECT_H
#define CAMERA_SELECT_H
#include <iostream>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <algorithm>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
#include <ros/ros.h>
#include <tf/tf.h>
#include <geometry_msgs/Pose.h>

#include <pcl/sample_consensus/sac_model_circle.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/pcl_base.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/point_cloud.h>

class CameraSelect
{
    public:
    CameraSelect();

    /********************对初始点云进行投影*********************/
    void poject_buc_select(cv::Mat image_point3d,cv::Mat image_depth, std::vector<std::map<int,int>>& average_dis, int n);
    /*********************对点云进行聚类***********************/
    void cluster_buc_select(cv::Mat depth,std::vector<std::map<int,int>>& average_dis,std::vector<std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>>& cloud_clusters_final);
    /*********************进行二次投影************************/
    void poject_buc_select(cv::Mat depth, std::vector<std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>>& cloud_clusters_final);
    /********************根据波动进行筛选**********************/
    void judge_point_dis(std::vector<std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>>& cloud_clusters_final);
    /********************根据点数进行筛选**********************/
    void judge_point_num(std::vector<std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>>& cloud_clusters_final);
    /**********************找到最近点*************************/
    void distance_buc(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters_buc,geometry_msgs::Pose& bucket2camera);
    /**********************综合相机检测************************/
    void camera_select(cv::Mat image_point3d,cv::Mat depth,geometry_msgs::Pose& bucket2camera,geometry_msgs::Pose& circle_center_point);

    /***********************聚类算法*************************/
    void cluster_buc(cv::Mat depth, std::map<int,int>& average_dis,std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters);
    /********************找到过中点的类*************************/
    void find_cluster(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_clusters,std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters_buc);

    /***********************圆弧拟合*************************/
    void circle_select(std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters,std::vector<Eigen::VectorXf> &model_circles);

    /***********************ronghe圆************************/
    void mix_circle(geometry_msgs::Pose& circle_center_point);

    void cluster_buc(cv::Mat depth, std::vector<std::map<int,int>>& average_dis,std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_clusters);

    private:
    int num_;
    int bucket_radius_;
    int average_lines_;
    ros::NodeHandle nh_private_;
    float circle_max_radius_;
    float circle_min_radius_;
    std::vector<std::vector<Eigen::VectorXf>> model_circles_series_;
};
#endif // CAMERA_SELECT_H
